
import math
from rclpy.node import Node
from sls_controller.sls_ugvInterface import UGVinterface
from sls_msgs.msg import UGVsteer,LlaVelocity

class UGVResource:
    # sigular instance
    _instance = None

    def __new__(cls, *args, **kwargs):
        if not cls._instance:
            cls._instance = super().__new__(cls)
        return cls._instance

    def __init__(self, main_node: Node, idStr: str):
        self.platform_interface = UGVinterface(main_node)
        self._idStr = idStr
        self.init_peripheral()

    # all peripheral init here
    def init_peripheral(self) -> None:
        pass

    # idStr
    def get_idStr(self) -> str:
        return self._idStr

    # get vehicle state
    def get_vehicle_speed_from_gps(self) -> float:
        velocity = self.platform_interface.vel
        if velocity is None:
            return 0.0  # TODO: handle None data
        return velocity  # vel

    def get_vehicle_speed_from_ugv(self) -> float:
        '''
            speed > 0 : D
            speed < 0 : R
        '''
        vehicle_speed = self.platform_interface.vehicle_speed
        vehicle_shiftlevel = self.get_vehicle_shiftlevel()
        if vehicle_speed is None and vehicle_shiftlevel != 0:
            return 0.0
        if vehicle_shiftlevel == 3:
            vehicle_speed = -vehicle_speed
        return vehicle_speed
    
    def get_vehicle_shiftlevel(self) -> int:
        '''
        1:D, 2:N, 3:R
        '''
        shiftlevel_sts = self.platform_interface.shiftlevel_sts
        if shiftlevel_sts is None:
            return 0
        return shiftlevel_sts

    def get_steering_wheel_direction(self) -> int:
        '''
        0 : left
        1 : right
        2 : None
        '''
        steering_wheel_direction = self.platform_interface.steering_wheel_direction
        if steering_wheel_direction is None:
            return 2
        return steering_wheel_direction

    def get_steering_angle(self) -> float:
        """
        get steering angle from vehicle
        unit: [rad]
        """
        steering_wheel_angle = self.platform_interface.steering_wheel_angle
        if steering_wheel_angle is None:
            return 0.0
        return steering_wheel_angle/120.0*27.0/180*3.14159265359

    def get_p_status(self) -> int:
        '''
        0 : release
        1 : apply
        2 : None
        '''        
        p_status = self.platform_interface.p_sts
        if p_status is None:
            return 2
        return p_status
    
    def get_drive_mode(self) -> int:
        '''
        0 : None
        1 : auto
        2 : PAD
        3 : steer-wheel
        '''
        drive_mode = self.platform_interface.drive_mode
        if drive_mode is None:
            return 0
        return drive_mode
    
    def get_gps_position(self) -> dict[str, float]|None:
        '''
            return {"latitude":latitude,"longitude":longitude,"altitude":altitude}
        '''
        # [lon, lat, alt]
        latitude = self.platform_interface.latitude
        longitude = self.platform_interface.longitude
        altitude = self.platform_interface.altitude
        if latitude is None or longitude is None or altitude is None:
            return None
        return {"latitude":latitude,"longitude":longitude,"altitude":altitude}
    
    def get_gps_yaw(self) -> float:
        '''
            NED (degree)
            0-360
        '''
        yaw = self.platform_interface.yaw
        if yaw is None:
            return 0. 
        return yaw

    def get_odom_position(self) -> dict:
        '''
        ENU
        return {x_posiyion, y_posiyion, z_posiyion}
        '''
        odom_position = self.platform_interface.odom_position
        return {"E":odom_position.x, "N":odom_position.y, "U":odom_position.z}
    def get_odom_quaternion(self) -> list:
        '''
        ENU
        return [x_quaternion, y_quaternion, z_quaternion, w_quaternion]
        '''
        odom_quaternion = self.platform_interface.odom_orientation
        return [odom_quaternion.x, odom_quaternion.y, odom_quaternion.z, odom_quaternion.w]

    def get_odom_linear_velocity(self) -> list:
        '''
        ENU
        return [x_velocity, y_velocity, z_velocity]
        '''
        odom_linear_velocity = self.platform_interface.odom_linear_velocity
        return {"E":odom_linear_velocity.x, "N":odom_linear_velocity.y, "U":odom_linear_velocity.z}

    def get_odom_angular_velocity(self) -> list:
        '''
        ENU
        [x_angular_velocity, y_angular_velocity, z_angular_velocity]
        '''
        odom_angular_velocity = self.platform_interface.odom_angular_velocity
        return {"E":odom_angular_velocity.x, "N":odom_angular_velocity.y, "U":odom_angular_velocity.z}
    
    # set vehicle control command
    def set_vehicle_cmd(self,
                        shift_level_reg:int=127,
                        target_speed:float=127.0,
                        steering_wheel_angle:float=127.0,
                        drive_mode_reg:int=127,
                        brk_en:int=127,
                        brake_mode:int=127,
                        acc_mode:int=127
    ) -> None:
        """
        param target_speed: speed of vehicle [km/s]
        param steering_wheel_angle: steering angle of vehicle [±120.0(0.1),±27°]
        param shift_level_reg: shift of vehicle [1:D, 2:N, 3:R]
        param brk_en: brake of vehicle [0:off, 1:on]
        param brake_mode: mode of break [1:default(100%, emergency break), 2:depend on Brake Coefficient, but I didn't write, you can add if you want]
        enum ShiftLevel : uint8_t { D = 1, N, R };
        enum drive_mode_reg : uint8_t { None = 0, Auto, PAD, Remote };
        """
        self.platform_interface.publish_chassis(self,
                                                shift_level_reg=shift_level_reg,
                                                target_speed=target_speed,
                                                steering_wheel_angle=steering_wheel_angle,
                                                drive_mode_reg=drive_mode_reg,
                                                brk_en=brk_en,
                                                brake_mode=brake_mode,
                                                acc_mode=acc_mode)

        return

    def set_vehicle_several_cmd(self,
                        speed_mps: float = 127.0,
                        steering_wheel_angle_deg: float = 127.0,
                        brk_en: int = 127,
    ) -> None:
        """
        param speed_mps: speed of vehicle [m/s] (-10m/s, 10m/s)
        param steering_wheel_angle_deg: angle of vehicle [degree](-27°, 27°)
        param brk_en: brake of vehicle [0:off, 1:on]
        """
        shift_level_reg = 127
        target_speed = 127.0
        steering_wheel_angle = 127.0
        drive_mode_reg = 127
        brake_mode = 1
        acc_mode = 127

        if steering_wheel_angle_deg < 125.0:
            # (-27° to 27° -> -120 to 120)
            if steering_wheel_angle_deg >27:
                steering_wheel_angle_deg=27
            if steering_wheel_angle_deg <-27:
                steering_wheel_angle_deg=-27
            steering_wheel_angle = (steering_wheel_angle_deg / 27.0) * 120.0
            drive_mode_reg = 1
            brk_en = 0

        if speed_mps < 125.0:
            brk_en = 0
            target_speed = speed_mps * 3.6
            drive_mode_reg = 1
            if speed_mps > 0:
                shift_level_reg = 1  # D
            elif speed_mps < 0:
                target_speed = -target_speed
                shift_level_reg = 3  # R
            else:
                shift_level_reg = 2  # N

        if 0 < brk_en < 125.0:
            brk_en = 1
            target_speed = 0.0
        else:
            brk_en = 0

        self.platform_interface.publish_chassis(
            shift_level_reg=shift_level_reg,
            target_speed=target_speed,
            steering_wheel_angle=-steering_wheel_angle,
            drive_mode_reg=drive_mode_reg,
            brk_en=brk_en,
            brake_mode=brake_mode,
            acc_mode=acc_mode
        )

    def set_simu_vehicle_cmd(self,
                        shift_level_reg:int=0,
                        target_speed:float=0.0,
                        steering_wheel_angle:float=0.0,
                        drive_mode_reg:int=0,
                        brk_en:int=0,
                        brake_mode:int=0,
                        acc_mode:int=0
        ) -> None:
            """
            param target_speed: speed of vehicle [km/s]
            param steering_wheel_angle: steering angle of vehicle [±120.0(0.1),±27°]
            param shift_level_reg: shift of vehicle [1:D, 2:N, 3:R]
            param brk_en: brake of vehicle [0:off, 1:on]
            param brake_mode: mode of break [1:default(100%, emergency break), 2:depend on Brake Coefficient, but I didn't write, you can add if you want]
            enum ShiftLevel : uint8_t { D = 1, N, R };
            enum drive_mode_reg : uint8_t { None = 0, Auto, PAD, Remote };
            """
            self.platform_interface.simu_public_classis(
                                                    shift_level_reg=shift_level_reg,
                                                    target_speed=target_speed/3.6,
                                                    steering_wheel_angle=steering_wheel_angle/120*27/180*3.14,
                                                    drive_mode_reg=drive_mode_reg,
                                                    brk_en=brk_en,
                                                    brake_mode=brake_mode,
                                                    acc_mode=acc_mode)

            return